//Debug data
#define DEBUG 0
#define BAUD 38400

// Define pin assignments
#define LEDPIN 13
#define THROTTLEPIN 2
#define ROLLPIN 3
#define PITCHPIN 4
#define YAWPIN 5
#define GEARPIN 6
#define AUXPIN 7

// Define transmitter commands
#define THROTTLETIMEOUT 25000
#define PULSETIMEOUT 4500
volatile int throttle = 9999;
volatile int roll = 9999;
volatile int pitch = 9999;
volatile int yaw = 9999;
volatile int gear = 9999;
volatile int aux= 9999;
unsigned long previousTime = 0;
unsigned long deltaTime = 0;
unsigned long nowTime = 0;

void setup() {
  pinMode(LEDPIN, OUTPUT);
  digitalWrite(LEDPIN, HIGH);
  pinMode(THROTTLEPIN, INPUT);
  pinMode(ROLLPIN, INPUT);
  pinMode(PITCHPIN, INPUT);
  pinMode(YAWPIN, INPUT);
  pinMode(GEARPIN, INPUT);
  pinMode(AUXPIN, INPUT);
  Serial.begin(BAUD);
  if (DEBUG == 0) attachInterrupt(1, readTransmitter, RISING); 
}

void loop() {
  nowTime = millis();
  if (DEBUG) readTransmitter();

  digitalWrite(LEDPIN, HIGH);
  Serial.print(deltaTime);
  Serial.print(",");
  Serial.print(throttle);
  Serial.print(",");
  Serial.print(roll);
  Serial.print(",");
  Serial.print(pitch);
  Serial.print(",");
  Serial.print(yaw);
  Serial.print(",");
  Serial.print(aux);
  Serial.print(",");
  Serial.println(gear);
  digitalWrite(LEDPIN, LOW);  
    
  Serial.println(millis() - nowTime);
}

void readTransmitter() {
  previousTime = millis();
  roll = pulseIn(ROLLPIN, HIGH, THROTTLETIMEOUT);
  aux = pulseIn(AUXPIN, HIGH, THROTTLETIMEOUT);
  pitch = pulseIn(PITCHPIN, HIGH, THROTTLETIMEOUT);
  yaw = pulseIn(YAWPIN, HIGH, THROTTLETIMEOUT);
  throttle = pulseIn(THROTTLEPIN, HIGH, THROTTLETIMEOUT); //why does throttle and aux read the same?
  gear = pulseIn(GEARPIN, HIGH, THROTTLETIMEOUT);
  deltaTime = millis() - previousTime;
}   
